
#include "cmd.h"
#include "main.h"
#include "drv832x.h"
#include "usr_flash.h"


typedef struct
{
    uint32_t foc_mode    : 3;
    uint32_t dri_ctl     : 2;
    uint32_t start_ctl   : 2;
    uint32_t study       : 2;
    uint32_t senser_type : 4;
    
    uint32_t reserve1    : 19;
    
    float position_tar;
    float speed_tar;
    float position_max;
    float speed_max;
    
}foc_ctl1_t;



typedef struct
{
    float position;
    float speed;
    float iq;
    fault_status_t fault_status;
}foc_controller_fb_t;





void can_cmd_ctl(uint16_t id, uint8_t *pdata)
{
    switch(id)
    {
        case 0x200:
        {
            foc_ctl1_t *foc_ctl1 = (foc_ctl1_t *)pdata;
            
            if(foc_ctl1->start_ctl != 0x3)
            {
                sys.foc_start = foc_ctl1->start_ctl;
            }
            if(foc_ctl1->dri_ctl != 0x3)
            {
                drv832x_out_ctl(foc_ctl1->dri_ctl);
            }
            if(foc_ctl1->foc_mode != 0x7)
            {
                sys.foc_type = (foc_type_t)foc_ctl1->foc_mode;
            }
            if(foc_ctl1->study != 0x3)
            {
                sys.foc_study = 1;
            }
            if(foc_ctl1->senser_type != 0xF)
            {
                sys.cfg.senser_type = (senser_type_t)foc_ctl1->senser_type;
                if(save_cfg_to_flash(&sys.cfg) == FLASH_COMPLETE)
                {
                    sys.fault_status.mild_fault.bit.flash_op_err = 0;
                }
                else
                {
                    sys.fault_status.mild_fault.bit.flash_op_err = 1; 	
                }
            }
            if(sys.foc_type == FOC_TYPE_POSITION)
            {
                sys.foc_controller.position_ref = foc_ctl1->position_tar;
            }
            if(sys.foc_type == FOC_TYPE_SPEED)
            {
                sys.foc_controller.speed_ref = foc_ctl1->speed_tar;
            }
            sys.pid_position.max = foc_ctl1->position_max;
            sys.pid_speed.max = foc_ctl1->speed_max;
            
        }break;
        case 0x300:
        {
            foc_controller_fb_t *foc_controller_fb = (foc_controller_fb_t*)pdata;
            if(sys.foc_type == FOC_TYPE_POSITION)
            {
                sys.foc_controller.position_ref = foc_controller_fb->position;
            }
        }break;
    }

}






